%===================================================================================================
%[]FUNCTION NAME: LambertTi.m
%[]AUTHOR: Jinsung Lee
%[]CREATED: 09/01/2017
%===================================================================================================
%[]FUNCTION DESCRIPTION:
%This function calculates the departure and arrival velocity vectors needed to transfer a satellite
%between two (2) points in space using a minimum delta-v trajectory.
%===================================================================================================
%[]INPUT VARIABLES:
%(R1)|Initial position.
%---------------------------------------------------------------------------------------------------
%(V1minus)|Initial orbital velocity.
%---------------------------------------------------------------------------------------------------
%(R2)|Final position.
%---------------------------------------------------------------------------------------------------
%(V2plus)|Final orbital velocity.
%---------------------------------------------------------------------------------------------------
%(u)|Central body gravitational parameter.
%===================================================================================================
%[]OUTPUT VARIABLES:
%(V1plus)|Departure velocity vector.
%---------------------------------------------------------------------------------------------------
%(V2minus)|Arrival velocity vector.
%---------------------------------------------------------------------------------------------------
%(dt)|Time of flight.
%===================================================================================================
%[]VARIABLE FORMAT:
%(R1)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V1minus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(R2)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V2plus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(u)|Scalar {1 x 1}.
%---------------------------------------------------------------------------------------------------
%(V1plus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V2minus)|Column Vector {3 x 1}.
%===================================================================================================
%[]AUXILIARY FUNCTIONS:
%(Rotation.m)|This function calculates the matrix needed to actively rotate a vector about an axis
%by a given angle using the Rodrigues rotation formula.
%===================================================================================================
%[]COMMENTS:
%None.
%===================================================================================================
function [V1plus,V2minus,dt] = LambertTi(R1,V1minus,R2,V2plus,u)
    
    r1 = norm(R1);
    %[km]Initial distance from the central body.
    
    r2 = norm(R2);
    %[km]Final distance from the central body.
    
    sigma = r1 / r2;
    %[]Ratio of the orbital ranges.
    
    phi = real(acos(dot(R1,R2) / (r1 * r2)));
    %[rad]Transfer angle.
    
    W = cross(R1,R2) / norm(cross(R1,R2));
    %[]Third direction in the perifocal coordinate system.
    
    if W(3) < 0;
        
        phi = 2 * pi - phi;
        %[rad]Transfer angle correction.
        
        W = -W;
        %[]Third direction in the PQW coordinate system correction.
        
    end
    
    emin = (1 - sigma) / sqrt(sigma^2 - 2 * sigma * cos(phi) + 1);
    %[]Minimum eccentricity for this transfer.
    
    Phase = atan2(sigma - cos(phi),sin(phi));
    %[rad]Phase angle.
    
    dx = 1000;
    %[]Grid density.
    
    if r1 < r2;
        
        theta = linspace(-Phase,pi-Phase,dx);
        %[rad]True anomaly range.
        
    else
        
        theta = linspace(pi-Phase,2*pi-Phase,dx);
        %[rad]True anomaly range.
        
    end
    
    V1plus = zeros(3,dx);
    %[km/s]Allocates memory for the departure velocity vectors.
    
    V2minus = zeros(3,dx);
    %[km/s]Allocates memory for the arrival velocity vectors.
    
    dv = zeros(1,dx);
    %[km/s]Allocates memory for the total mission velocity change magnitude.
    
    for k = 1:dx;
        
        e = emin / sin(theta(k) + Phase);
        %[rad]Current eccentricity.
        
        p = r1 * (1 + e * cos(theta(k)));
        %[km]Current semiparameter.
        
        Rot = Rotation(W,-theta(k),'Radians');
        %[]Matrix that rotates a vector about the W-axis by an angle -theta.
        
        P = Rot * R1 / r1;
        %[]Principle direction in the perifocal coordinate system.
        
        Q = cross(W,P);
        %[]Secondary direction in the perifocal coordinate system.
        
        PqwToEci = [P, Q, W];
        %[]Matrix needed to transform vectors from PQW coordinates to ECI coordiantes.
        
        V1plus(:,k) = sqrt(u / p) * PqwToEci * [-sin(theta(k)); cos(theta(k)) + e; 0];
        %[km/s]Current departure velocity.
        
        V2minus(:,k) = sqrt(u / p) * PqwToEci * [-sin(theta(k) + phi); cos(theta(k) + phi) + e; 0];
        %[km/s]Current arrival velocity.
        
        dv1 = norm(V1plus(:,k) - V1minus);
        %[km/s]Current departure change in speed.
        
        dv2 = norm(V2plus - V2minus(:,k));
        %[km/s]Current arrival change in speed.
        
        dv(k) = dv1 + dv2;
        %[km/s]Current total mission change in speed.
        
    end
    
    [~,index] = min(dv);
    %[km/s,-]Determines the index of the optimal total mission speed change magnitude.
    
    theta = theta(index);
    %[rad]Optimal departure true anomaly.
    
    e = emin / sin(theta + Phase);
    %[]Optimal transfer orbit eccentricity.
    
    p = r1 * (1 + e * cos(theta));
    %[km]Current semiparameter.
    
    E1 = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(theta / 2));
    %[rad]Optimal transfer orbit departure eccentric anomaly.
    
    E2 = 2 * atan(sqrt((1 - e) / (1 + e)) * tan((theta + phi) / 2));
    %[rad]Optimal transfer orbit arrival eccentric anomaly.
    
    a = p / (1 - e^2);
    %[km]Optimal transfer orbit semimajor axis.
    
    n = sqrt(u / a^3);
    %[rad/s]Optimal transfer orbit mean angular motion.
    
    t1 = (E1 - e * sin(E1)) / n;
    %[s]Optimal transfer orbit departure time.
    
    t2 = (E2 - e * sin(E2)) / n;
    %[s]Optimal transfer orbit arrival time.
    
    dt = t2 - t1;
    %[s]Optimal transfer time of flight.
    
    if dt < 0;
        
        T = 2 * pi / n;
        %[s]Optimal transfer orbital period.
        
        dt = dt + T;
        %[s]Time of flight correction.
        
    end
    
    V1plus = V1plus(:,index);
    %[km/s]Optimal departure velocity vector.
    
    V2minus = V2minus(:,index);
    %[km/s]Optimal arrival velocity vector.
    
end
%===================================================================================================